/*
 * velocity.c
 *
 *  Created on: May 21, 2021
 *      Author: kirk_
 */

#include "velocity.h"

void VelocityThread(void* argument)
{
	//uint8_t i = 0;

	/* Init motors */
	KDL_TB6612_Init(&Motor[0]);
	KDL_TB6612_SetPort(&Motor[0], &htim15, TIM_CHANNEL_1, KDL_TB6612_PortComplement);
	KDL_TB6612_SetPin1(&Motor[0], A_IN1_GPIO_Port, A_IN1_Pin);
	KDL_TB6612_SetPin2(&Motor[0], A_IN2_GPIO_Port, A_IN2_Pin);
	KDL_TB6612_SetSpeed(&Motor[0], -0.2);

	KDL_TB6612_Init(&Motor[1]);
	KDL_TB6612_SetPort(&Motor[1], &htim15, TIM_CHANNEL_2, KDL_TB6612_PortGeneral);
	KDL_TB6612_SetPin1(&Motor[1], B_IN1_GPIO_Port, B_IN1_Pin);
	KDL_TB6612_SetPin2(&Motor[1], B_IN2_GPIO_Port, B_IN2_Pin);
	KDL_TB6612_SetSpeed(&Motor[1], -0.2);

	KDL_TB6612_Init(&Motor[2]);
	KDL_TB6612_SetPort(&Motor[2], &htim5, TIM_CHANNEL_3, KDL_TB6612_PortGeneral);
	KDL_TB6612_SetPin1(&Motor[2], C_IN1_GPIO_Port, C_IN1_Pin);
	KDL_TB6612_SetPin2(&Motor[2], C_IN2_GPIO_Port, C_IN2_Pin);
	KDL_TB6612_SetSpeed(&Motor[2], -0.2);

	KDL_TB6612_Init(&Motor[3]);
	KDL_TB6612_SetPort(&Motor[3], &htim5, TIM_CHANNEL_4, KDL_TB6612_PortGeneral);
	KDL_TB6612_SetPin1(&Motor[3], D_IN1_GPIO_Port, D_IN1_Pin);
	KDL_TB6612_SetPin2(&Motor[3], D_IN2_GPIO_Port, D_IN2_Pin);
	KDL_TB6612_SetSpeed(&Motor[3], -0.2);

	/* Test motor */


	osThreadExit();
}
